import time
from machine import Pin,ADC,TouchPad
import _thread
blue=Pin(21,Pin.OUT)
green=Pin(22,Pin.OUT)
red=Pin(23,Pin.OUT)
p1=Pin(14,Pin.OUT)        
d1=Pin(12,Pin.OUT)
e1=Pin(13,Pin.OUT)
p2=Pin(15,Pin.OUT)        
d2=Pin(2,Pin.OUT)
e2=Pin(0,Pin.OUT)
adc1=ADC(Pin(34))
adc2=ADC(Pin(35))
touch = TouchPad(Pin(27))
eep=Pin(26,Pin.OUT)        

def read_plus(dianji):
  if dianji == 1:
    angle = adc1.read()
  if dianji == 2:
    angle = adc2.read()
  if angle<270:
    angle = 270
  if angle>3020:
    angle = 3020
  angle = (angle-270)*180/2750
  return int(angle*20/9)
def beep(run_time,stop_time,count):
  for i in range(count):
    eep.value(1)
    time.sleep(run_time)
    eep.value(0)
    time.sleep(stop_time)    
def read_touch():
  return touch.read()
def color_led(r,b,g):
  red.value(r)
  blue.value(b)
  green.value(g)
def enable(a,b):
  e1.value(a)
  e2.value(b)
def recode():
  f = open('log.txt','w')
  a = read_touch()
  while a<300:
    a = read_touch()
    f.write(str(read_plus(1))+','+str(read_plus(2))+';')
    time.sleep_us(50000)
  f.close()
def run(position_1,position_2):
  last_position_1 = 0
  last_position_2 = 0
  for i in range(len(position_1)):
    step_1 = abs(int(position_1[i]) - last_position_1)
    dir_1  = 0 if (int(position_1[i]) > last_position_1) else 1
    last_position_1 = int(position_1[i])
    step_2 = abs(int(position_2[i]) - last_position_2)
    dir_2  = 0 if (int(position_2[i]) > last_position_2) else 1
    last_position_2 = int(position_2[i])
    d1.value(dir_1) 
    d2.value(dir_2) 
    p1.value(0)
    p2.value(0)
    delay_time_1 = int(50000/step_1) if step_1 != 0 else 50000
    delay_time_2 = int(50000/step_2) if step_2 != 0 else 50000
    gcd_time = 100
    plus_1 = int(delay_time_1/gcd_time)
    plus_2 = int(delay_time_2/gcd_time)
    q = 1
    for i in range(50000/gcd_time):
      if (q%plus_1) == 0 and plus_1 > 5: 
        p1.value(1)
      if (q%plus_2) == 0: 
        p2.value(1)
      half_gcd_time = int(gcd_time/2)
      time.sleep_us(half_gcd_time)
      p1.value(0)
      p2.value(0)
      q += 1
      time.sleep_us(half_gcd_time) 
      
def reproduce():
  f = open('log.txt','r') 
  text = f.read()
  text_list = text.split(';')[:-1]
  position_1=[]
  position_2=[]
  for i in text_list:
    position_1.append(i.split(',')[0])
    position_2.append(i.split(',')[1])
  print('position_1')
  print(position_1)
  print('position_2')
  print(position_2)
  beep(0.2,1,1)
  run(position_1,position_2)
  f.close()
  
color_led(1,0,0)
time.sleep(0.5) 
color_led(0,1,0)
time.sleep(0.5)
color_led(0,0,1)
time.sleep(0.5)
color_led(0,0,0)
enable(0,0)
while True:
  while read_touch()>250:
    color_led(1,0,0)
  color_led(0,0,0)
  recode()
  reproduce()





